#include <iostream>
#include <stdio.h>
#include "mjbot_spi_interface.hpp"

int main(void)
{
    float pos;
    float vel;
    float torque;
    uint16_t id;
    Mjbot_SPI_Interface mjbot_spi("/dev/spidev4.1", 8000000, 0, 8, 0);

    mjbot_spi.mjbots_control(1, 0x8001, 0, 1, 0, 1, 1);
    sleep(0.5);
    if(0 == mjbot_spi.mjbots_feedback(1, &id, &pos, &vel, &torque))
    {
        printf("id: %x, position: %f, velocity: %f, torque: %f\n", id, pos, vel, torque);
    }    
}
